<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvmath/qvukf.cpp</h1><a href="qvukf_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*</span>
<a name="l00002"></a>00002 <span class="comment"> *      Copyright (C) 2010, 2011, 2012. PARP Research Group.</span>
<a name="l00003"></a>00003 <span class="comment"> *      &lt;http://perception.inf.um.es&gt;</span>
<a name="l00004"></a>00004 <span class="comment"> *      University of Murcia, Spain.</span>
<a name="l00005"></a>00005 <span class="comment"> *</span>
<a name="l00006"></a>00006 <span class="comment"> *      This file is part of the QVision library.</span>
<a name="l00007"></a>00007 <span class="comment"> *</span>
<a name="l00008"></a>00008 <span class="comment"> *      QVision is free software: you can redistribute it and/or modify</span>
<a name="l00009"></a>00009 <span class="comment"> *      it under the terms of the GNU Lesser General Public License as</span>
<a name="l00010"></a>00010 <span class="comment"> *      published by the Free Software Foundation, version 3 of the License.</span>
<a name="l00011"></a>00011 <span class="comment"> *</span>
<a name="l00012"></a>00012 <span class="comment"> *      QVision is distributed in the hope that it will be useful,</span>
<a name="l00013"></a>00013 <span class="comment"> *      but WITHOUT ANY WARRANTY; without even the implied warranty of</span>
<a name="l00014"></a>00014 <span class="comment"> *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span>
<a name="l00015"></a>00015 <span class="comment"> *      GNU Lesser General Public License for more details.</span>
<a name="l00016"></a>00016 <span class="comment"> *</span>
<a name="l00017"></a>00017 <span class="comment"> *      You should have received a copy of the GNU Lesser General Public</span>
<a name="l00018"></a>00018 <span class="comment"> *      License along with QVision. If not, see &lt;http://www.gnu.org/licenses/&gt;.</span>
<a name="l00019"></a>00019 <span class="comment"> */</span>
<a name="l00020"></a>00020 
<a name="l00024"></a>00024 
<a name="l00025"></a>00025 <span class="preprocessor">#include &lt;<a class="code" href="qvukf_8h.html" title="File from the QVision library.">qvmath/qvukf.h</a>&gt;</span>
<a name="l00026"></a><a class="code" href="classQVUKF.html#a77a7d85376b98d7d6596c8ff9b72548f">00026</a> <a class="code" href="classQVUKF.html#a03c50b23aa537c66bdf42c98f54611df" title="QVUKF default constructor.">QVUKF::QVUKF</a>(<span class="keyword">const</span> <a class="code" href="structQVUKFstate.html" title="A structure that contains the mean and covariance matrix to represent a normal state...">QVUKFstate</a> &amp;state)
<a name="l00027"></a>00027 {
<a name="l00028"></a>00028         currentState = state;
<a name="l00029"></a>00029         n = state.<a class="code" href="structQVUKFstate.html#a0702c9a81f1f3bd67426c02e7a5ad1e2" title="Filter mean.">mean</a>.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>();
<a name="l00030"></a>00030 
<a name="l00031"></a>00031         <span class="comment">//default values</span>
<a name="l00032"></a>00032         k = 0;
<a name="l00033"></a>00033         alpha = 0.5;
<a name="l00034"></a>00034         beta = 2;
<a name="l00035"></a>00035         discrepance = 0;
<a name="l00036"></a>00036 }
<a name="l00037"></a>00037 
<a name="l00038"></a><a class="code" href="classQVUKF.html#a473451fad3c9b2016fed435a9902e2c7">00038</a> <span class="keywordtype">void</span> <a class="code" href="classQVUKF.html#a473451fad3c9b2016fed435a9902e2c7" title="Method that fills the current state from a given QVUKFstate.">QVUKF::setState</a>(<span class="keyword">const</span> <a class="code" href="structQVUKFstate.html" title="A structure that contains the mean and covariance matrix to represent a normal state...">QVUKFstate</a> &amp;state)
<a name="l00039"></a>00039 {
<a name="l00040"></a>00040         currentState = state;
<a name="l00041"></a>00041         n = state.<a class="code" href="structQVUKFstate.html#a0702c9a81f1f3bd67426c02e7a5ad1e2" title="Filter mean.">mean</a>.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>();
<a name="l00042"></a>00042         discrepance = 0;
<a name="l00043"></a>00043 }
<a name="l00044"></a>00044 
<a name="l00045"></a><a class="code" href="classQVUKF.html#a0eb739a92938bd540883c462cf0e8428">00045</a> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> <a class="code" href="classQVUKF.html#a0eb739a92938bd540883c462cf0e8428" title="Method that returns the current system state.">QVUKF::getState</a>()<span class="keyword"> const</span>
<a name="l00046"></a>00046 <span class="keyword"></span>{
<a name="l00047"></a>00047         <span class="keywordflow">return</span> currentState.<a class="code" href="structQVUKFstate.html#a0702c9a81f1f3bd67426c02e7a5ad1e2" title="Filter mean.">mean</a>;
<a name="l00048"></a>00048 }
<a name="l00049"></a>00049 
<a name="l00050"></a><a class="code" href="classQVUKF.html#af4c53f693838f00936798fd63ec0b07b">00050</a> <span class="keywordtype">double</span> <a class="code" href="classQVUKF.html#af4c53f693838f00936798fd63ec0b07b" title="Method that returns the discrepancy.">QVUKF::getDiscrepance</a>()<span class="keyword"> const</span>
<a name="l00051"></a>00051 <span class="keyword"></span>{
<a name="l00052"></a>00052         <span class="keywordflow">return</span> discrepance;
<a name="l00053"></a>00053 }
<a name="l00054"></a>00054 
<a name="l00055"></a>00055 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> QVUKF::sigmaPoints(<span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;X, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;P)<span class="keyword"> const</span>
<a name="l00056"></a>00056 <span class="keyword"></span>{
<a name="l00057"></a>00057         <span class="keywordtype">double</span> lambda = (alpha*alpha) * (n+k) - n;
<a name="l00058"></a>00058         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> aux = P * (n + lambda);
<a name="l00059"></a>00059         <span class="comment">//Auxiliar matrix for Cholesky decomposition</span>
<a name="l00060"></a>00060         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> aux2 = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>();
<a name="l00061"></a>00061 
<a name="l00062"></a>00062         <span class="comment">//Calling to Cholesky decomposition</span>
<a name="l00063"></a>00063         <a class="code" href="group__qvmatrixalgebra.html#gab845aa50ce874199ad7676904829b932" title="Obtains the Cholesky decomposition of a symmetric and positive definite matrix.">CholeskyDecomposition</a>(aux, aux2);
<a name="l00064"></a>00064         aux2 = aux2.transpose();
<a name="l00065"></a>00065         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> Sigma = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(2*n + 1, n, 0.0);
<a name="l00066"></a>00066         <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> Xvector = <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(X);
<a name="l00067"></a>00067         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;2*n+1; i++)
<a name="l00068"></a>00068         {
<a name="l00069"></a>00069                 <span class="keywordflow">if</span>(i==0)
<a name="l00070"></a>00070                         Sigma.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, Xvector);
<a name="l00071"></a>00071                 <span class="keywordflow">if</span> (i&gt;0 &amp;&amp; i&lt;=n)
<a name="l00072"></a>00072                         Sigma.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, X + aux2.<a class="code" href="classQVMatrix.html#ae27f08cd9a56a5c85ba2757cc4078d80" title="Gets a row of the matrix.">getRow</a>(i-1));
<a name="l00073"></a>00073                 <span class="keywordflow">if</span> (i&gt;n &amp;&amp; i&lt;2*n+1)
<a name="l00074"></a>00074                         Sigma.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, X - aux2.<a class="code" href="classQVMatrix.html#ae27f08cd9a56a5c85ba2757cc4078d80" title="Gets a row of the matrix.">getRow</a>(i-n-1));
<a name="l00075"></a>00075         }
<a name="l00076"></a>00076 
<a name="l00077"></a>00077         <span class="keywordflow">return</span> Sigma;
<a name="l00078"></a>00078 }
<a name="l00079"></a>00079 
<a name="l00080"></a>00080 <span class="keywordtype">void</span> QVUKF::computeWeights()
<a name="l00081"></a>00081 {
<a name="l00082"></a>00082         <span class="keywordtype">double</span> lambda = pow(alpha, 2)*(n+k)-n;
<a name="l00083"></a>00083         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> Wm = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(2*n+1, 1);
<a name="l00084"></a>00084         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> Wc = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(2*n+1, 1);
<a name="l00085"></a>00085 
<a name="l00086"></a>00086         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;2*n+1; i++)
<a name="l00087"></a>00087         {
<a name="l00088"></a>00088                 <span class="keywordflow">if</span>(i==0)
<a name="l00089"></a>00089                 {
<a name="l00090"></a>00090                         <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> x = <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(1,lambda/(n+lambda));
<a name="l00091"></a>00091                         <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> y = <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(1,(lambda/(n+lambda))+(1.0-pow(alpha,2)+beta));
<a name="l00092"></a>00092                         Wm.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, x);
<a name="l00093"></a>00093                         Wc.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, y);
<a name="l00094"></a>00094                 }
<a name="l00095"></a>00095 
<a name="l00096"></a>00096                 <span class="keywordflow">if</span> (i&gt;0)
<a name="l00097"></a>00097                 {
<a name="l00098"></a>00098                         <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> z = <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(1,1.0/(2.0*(n+lambda)));
<a name="l00099"></a>00099                         Wm.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, z);
<a name="l00100"></a>00100                         Wc.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, z);
<a name="l00101"></a>00101                 }
<a name="l00102"></a>00102         }
<a name="l00103"></a>00103 
<a name="l00104"></a>00104         this-&gt;weights.<a class="code" href="structQVUKFweights.html#a7f794a47ae11c4d3be80c9c5c847e656" title="Weight matrix for the filter parameters corresponding to the mean.">Wm</a> = Wm;
<a name="l00105"></a>00105         this-&gt;weights.<a class="code" href="structQVUKFweights.html#afca2849d6d590e4d0db1be63b147cb51" title="Weight matrix for the filter parameters corresponding to the covariance.">Wc</a> = Wc;
<a name="l00106"></a>00106 }
<a name="l00107"></a>00107 
<a name="l00108"></a>00108 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> QVUKF::computeMean(<span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;M)<span class="keyword"> const</span>
<a name="l00109"></a>00109 <span class="keyword"></span>{
<a name="l00110"></a>00110         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> mean = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(1, M.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), 0.0);
<a name="l00111"></a>00111         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j=0; j&lt;M.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(); j++)
<a name="l00112"></a>00112         {
<a name="l00113"></a>00113                 <span class="keywordtype">double</span> accum =0;
<a name="l00114"></a>00114                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;2*n+1; i++)
<a name="l00115"></a>00115                         accum += weights.<a class="code" href="structQVUKFweights.html#a7f794a47ae11c4d3be80c9c5c847e656" title="Weight matrix for the filter parameters corresponding to the mean.">Wm</a>(i,0)*M(i,j);
<a name="l00116"></a>00116                 <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> v = <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(1, (accum));
<a name="l00117"></a>00117                 mean.<a class="code" href="classQVMatrix.html#a8ebc03603ced1c727afd6f3bd755e57a" title="Sets a column of the matrix.">setCol</a>(j, v);
<a name="l00118"></a>00118         }
<a name="l00119"></a>00119 
<a name="l00120"></a>00120         <span class="keywordflow">return</span> mean;
<a name="l00121"></a>00121 }
<a name="l00122"></a>00122 
<a name="l00123"></a>00123 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> QVUKF::computeCovariance(<span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;mean, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;FdeSigma)<span class="keyword"> const</span>
<a name="l00124"></a>00124 <span class="keyword"></span>{
<a name="l00125"></a>00125         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> covariance = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(mean.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), mean.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), 0.0);
<a name="l00126"></a>00126 
<a name="l00127"></a>00127         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;2*n+1; i++)
<a name="l00128"></a>00128         {
<a name="l00129"></a>00129                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> accum = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(mean.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), mean.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), 0.0);
<a name="l00130"></a>00130                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> YXprima = FdeSigma.<a class="code" href="classQVMatrix.html#ae27f08cd9a56a5c85ba2757cc4078d80" title="Gets a row of the matrix.">getRow</a>(i)-mean;
<a name="l00131"></a>00131                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> YXprimaTrans = YXprima.<a class="code" href="classQVMatrix.html#a2f87710c9d8ae4b07b03605daea3782e" title="Change the order of the indexes in the matrix.">transpose</a>();
<a name="l00132"></a>00132                 accum = YXprimaTrans*YXprima;
<a name="l00133"></a>00133                 accum = accum.operator*(weights.<a class="code" href="structQVUKFweights.html#afca2849d6d590e4d0db1be63b147cb51" title="Weight matrix for the filter parameters corresponding to the covariance.">Wc</a>(i, 0));
<a name="l00134"></a>00134                 covariance = covariance + accum;
<a name="l00135"></a>00135         }
<a name="l00136"></a>00136 
<a name="l00137"></a>00137         <span class="keywordflow">return</span> covariance;
<a name="l00138"></a>00138 }
<a name="l00139"></a>00139 
<a name="l00140"></a>00140 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> QVUKF::CrossCovariance (<span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;mean, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;FdeSigma, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;zt, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Zt)<span class="keyword"> const</span>
<a name="l00141"></a>00141 <span class="keyword"></span>{
<a name="l00142"></a>00142 
<a name="l00143"></a>00143         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> covariance = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(n, zt.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), 0.0);
<a name="l00144"></a>00144 
<a name="l00145"></a>00145         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;2*n+1; i++)
<a name="l00146"></a>00146         {
<a name="l00147"></a>00147                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> accum = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(n, zt.<a class="code" href="classQVMatrix.html#a4108aa685baecab8a9822dcc04e98b7f" title="Get height of the matrix.">getRows</a>(), 0.0);
<a name="l00148"></a>00148                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> YXprima = FdeSigma.<a class="code" href="classQVMatrix.html#ae27f08cd9a56a5c85ba2757cc4078d80" title="Gets a row of the matrix.">getRow</a>(i) - mean;
<a name="l00149"></a>00149                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> zZprima = Zt.<a class="code" href="classQVMatrix.html#ae27f08cd9a56a5c85ba2757cc4078d80" title="Gets a row of the matrix.">getRow</a>(i) - zt;
<a name="l00150"></a>00150 
<a name="l00151"></a>00151                 accum = YXprima.<a class="code" href="classQVMatrix.html#a2f87710c9d8ae4b07b03605daea3782e" title="Change the order of the indexes in the matrix.">transpose</a>()*zZprima;
<a name="l00152"></a>00152                 accum = accum.operator*(weights.<a class="code" href="structQVUKFweights.html#afca2849d6d590e4d0db1be63b147cb51" title="Weight matrix for the filter parameters corresponding to the covariance.">Wc</a>(i,0));
<a name="l00153"></a>00153                 covariance = covariance + accum;
<a name="l00154"></a>00154         }
<a name="l00155"></a>00155 
<a name="l00156"></a>00156         <span class="keywordflow">return</span> covariance;
<a name="l00157"></a>00157 
<a name="l00158"></a>00158 }
<a name="l00159"></a>00159 
<a name="l00160"></a><a class="code" href="classQVUKF.html#a2a8d8d4b08c95657876036a7b337280e">00160</a> <span class="keywordtype">void</span> <a class="code" href="classQVUKF.html#a2a8d8d4b08c95657876036a7b337280e" title="Method that updates the current state, moving from X_k to X_k+1.">QVUKF::update</a>(<span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;obs, <a class="code" href="classQVFunction.html">QVFunction&lt;QVVector, QVVector&gt;</a> &amp;g, <a class="code" href="classQVFunction.html">QVFunction&lt;QVVector, QVVector&gt;</a> &amp;h, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Rt, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Qt)
<a name="l00161"></a>00161 {
<a name="l00162"></a>00162         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> SigmaPoints = sigmaPoints(currentState.<a class="code" href="structQVUKFstate.html#a0702c9a81f1f3bd67426c02e7a5ad1e2" title="Filter mean.">mean</a>, currentState.<a class="code" href="structQVUKFstate.html#a697a28c3c6d5cf71c6951f251b7d3287" title="Filter covariance matrix.">covariance</a>);
<a name="l00163"></a>00163         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> FdeSigma = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(2*n+1, n, 0.0);
<a name="l00164"></a>00164 
<a name="l00165"></a>00165         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;2*n+1; i++)
<a name="l00166"></a>00166         {
<a name="l00167"></a>00167                 <span class="comment">//Applying the dynamic model function g()</span>
<a name="l00168"></a>00168                 <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> aux = SigmaPoints.<a class="code" href="classQVMatrix.html#ae27f08cd9a56a5c85ba2757cc4078d80" title="Gets a row of the matrix.">getRow</a>(i);
<a name="l00169"></a>00169                 FdeSigma.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, g.<a class="code" href="classQVFunction.html#a547d70580fc6ca0c97cb299958f8c85d" title="Virtual method implementing the code for the function evaluation.">evaluate</a>(aux));
<a name="l00170"></a>00170         }
<a name="l00171"></a>00171 
<a name="l00172"></a>00172         computeWeights();
<a name="l00173"></a>00173         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> mean_t = computeMean(FdeSigma);
<a name="l00174"></a>00174         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> covar_t = computeCovariance(mean_t, FdeSigma) + Rt;
<a name="l00175"></a>00175         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> SigmaPoints_t = sigmaPoints(mean_t, covar_t);
<a name="l00176"></a>00176 
<a name="l00177"></a>00177         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> Zt = <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>(2*n+1, obs.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), 0.0);
<a name="l00178"></a>00178         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i=0; i&lt;2*n+1; i++)
<a name="l00179"></a>00179         {
<a name="l00180"></a>00180                 <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> aux = SigmaPoints_t.<a class="code" href="classQVMatrix.html#ae27f08cd9a56a5c85ba2757cc4078d80" title="Gets a row of the matrix.">getRow</a>(i);
<a name="l00181"></a>00181                 Zt.<a class="code" href="classQVMatrix.html#ad8684520b5095fb350022fc9acb0dba8" title="Sets a row of the matrix.">setRow</a>(i, h.<a class="code" href="classQVFunction.html#a547d70580fc6ca0c97cb299958f8c85d" title="Virtual method implementing the code for the function evaluation.">evaluate</a>(aux));
<a name="l00182"></a>00182         }
<a name="l00183"></a>00183 
<a name="l00184"></a>00184         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> zt = computeMean(Zt);
<a name="l00185"></a>00185         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> St = computeCovariance(zt, Zt) + Qt;
<a name="l00186"></a>00186         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> CrossCov = CrossCovariance(mean_t, FdeSigma, zt, Zt);
<a name="l00187"></a>00187 
<a name="l00188"></a>00188         <span class="comment">//Kalman gain</span>
<a name="l00189"></a>00189         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> St_inv(St.<a class="code" href="classQVMatrix.html#a4108aa685baecab8a9822dcc04e98b7f" title="Get height of the matrix.">getRows</a>(), St.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>());
<a name="l00190"></a>00190         SolveByCholeskyDecomposition(St, St_inv, <a class="code" href="classQVMatrix.html#a6eb15980d17c17d4a55ab09a77bcd715" title="Creates an identity matrix.">QVMatrix::identity</a>(St.<a class="code" href="classQVMatrix.html#a4108aa685baecab8a9822dcc04e98b7f" title="Get height of the matrix.">getRows</a>()));
<a name="l00191"></a>00191         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> Kt = CrossCov * St_inv;
<a name="l00192"></a>00192         <span class="comment">//QVMatrix Kt = CrossCov * St.inverse();</span>
<a name="l00193"></a>00193 
<a name="l00194"></a>00194         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> dis = obs - zt;
<a name="l00195"></a>00195         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> final_mean = mean_t + (dis)*Kt.<a class="code" href="classQVMatrix.html#a2f87710c9d8ae4b07b03605daea3782e" title="Change the order of the indexes in the matrix.">transpose</a>();
<a name="l00196"></a>00196         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> final_covariance = covar_t - Kt*St*Kt.<a class="code" href="classQVMatrix.html#a2f87710c9d8ae4b07b03605daea3782e" title="Change the order of the indexes in the matrix.">transpose</a>();
<a name="l00197"></a>00197 
<a name="l00198"></a>00198         <span class="comment">//Updating current state (mu, sigma)</span>
<a name="l00199"></a>00199         this-&gt;currentState.<a class="code" href="structQVUKFstate.html#a0702c9a81f1f3bd67426c02e7a5ad1e2" title="Filter mean.">mean</a> = final_mean;
<a name="l00200"></a>00200         this-&gt;currentState.<a class="code" href="structQVUKFstate.html#a697a28c3c6d5cf71c6951f251b7d3287" title="Filter covariance matrix.">covariance</a> = final_covariance;
<a name="l00201"></a>00201         this-&gt;discrepance = dis.<a class="code" href="classQVMatrix.html#a387750529622104faba8047cb0efa62a" title="Gets the norm2 for matrix.">norm2</a>();
<a name="l00202"></a>00202 }
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
